px_d = out.px_d.data;
py_d = out.py_d.data;
pz_d = out.pz_d.data;
px = out.px.data;
py = out.py.data;
pz = out.pz.data;
q = out.q.data;

figure(1);
p1 = plot3(px_d, py_d, pz_d);
hold on;
p2 = plot3(px, py, pz, '--');
legend([p1, p2], {'desired', 'actual'}, 'AutoUpdate', 'off');
zlim([-1.2, 0.8])
robot_real.plot(q);
